package FlowGram.Application.Parser.Controllers;


import java.awt.Color;

import org.apache.commons.configuration.ConfigurationException;
import FlowGram.Application.Parser.Utility.Configuration;

import edu.cmu.ri.createlab.terk.robot.finch.Finch;


/**
 * 
 * @author thackray
 *
 */
public class FinchRobotController extends FinchController {

	protected Finch finch;
	protected Configuration config;
	
	protected double orientation;
	
	public FinchRobotController() throws Exception{
		
		try {
			this.finch = new Finch();
			this.config = Configuration.getInstance();
		} catch (ConfigurationException e) {
			throw new Exception("Unable to load configuration!\n"+e.getMessage());
		}/*catch(Exception e){
			throw new Exception("Unable to connect to Finch. "+e.getMessage());
		}*/

	}
	public boolean isConnected(){
		return this.finch == null;
	}
	public void destruct(){
		this.finch.quit();
	}
	protected void printOrientation(){
		System.out.println("Finch orientation = "+(this.orientation*(180/Math.PI))+" degrees");
	}
	
	public String getName(){
		return "FinchRobotController";
	}
	
	public void resetState(){
		this.orientation = 0;
	}
	
	@Override
	public void moveForward() {
		finch.setWheelVelocities(this.config.config.getInt("robot.movevelocity"),
								 this.config.config.getInt("robot.movevelocity"),
								 this.config.config.getInt("robot.movetime"));
	}

	@Override
	public void turnLeft() {
		int currTime = 0;
		while(currTime < this.config.config.getInt("robot.turntime")){
			finch.setWheelVelocities(this.config.config.getInt("robot.turnvelocity")*-1,
					 this.config.config.getInt("robot.turnvelocity"),
					 10);
			currTime += 10;
			this.orientation += this.getAngle(finch.getXAcceleration(), finch.getYAcceleration());
		}
		/*finch.setWheelVelocities(this.config.config.getInt("robot.turnvelocity")*-1,
								 this.config.config.getInt("robot.turnvelocity"),
								 this.config.config.getInt("robot.turntime"));
		this.orientation += this.getAngle(finch.getXAcceleration(), finch.getYAcceleration());*/
		this.printOrientation();
	}

	@Override
	public void turnRight() {
		finch.setWheelVelocities(this.config.config.getInt("robot.turnvelocity"),
								 this.config.config.getInt("robot.turnvelocity")*-1,
								 this.config.config.getInt("robot.turntime"));
		this.orientation += this.getAngle(finch.getXAcceleration(), finch.getYAcceleration());
		this.printOrientation();
	}
	
	public void setColor(Color c){
		finch.setLED(c);
	}
	public void setColor(Color c, int time){
		finch.setLED(c, time);
	}

	@Override
	public boolean isBlocked() {
		return finch.isObstacle();
	}

	@Override
	public boolean isTapped() {
		return finch.isTapped();
	}

	@Override
	public void chirp(int frequency, int duration) {
		finch.buzz(frequency, duration);
		
	}
	
	protected double getAngle(double x, double y){
		return Math.atan2(y, -x);
		
	}
	@Override
	public boolean isOnBeeper() {
		// TODO Auto-generated method stub
		return false;
	}
	@Override
	public void pickBeeper() {
		// TODO Auto-generated method stub
		
	}
	@Override
	public void putBeeper(Color c) {
		// TODO Auto-generated method stub
		
	}

}
